#include "Matrix.h"

/********************************************
/*              Public
/********************************************/

    /********************************************
    /*              Constructors
    /********************************************/

        Matrix4::Matrix4(){
            m_Datas = new float[16];
            Identity();
        }

        Matrix4::Matrix4(float * array){
            m_Datas= new float[16];
            std::copy(array, array+16, m_Datas);
        }

        Matrix4::Matrix4(const Matrix4 & mat){
            m_Datas = mat.Data();
        }

        Matrix4::~Matrix4(){
            delete m_Datas;
        }

    /********************************************
    /*          Get/Set
    /********************************************/

        float   Matrix4::at(int i) const{
            if(i<16){
                return m_Datas[i];
            }
            return -1.0f;
        }

    /********************************************
    /*          Operator Overload
    /********************************************/

        float & Matrix4::operator[](int pos){
            return m_Datas[pos];
        }

        void Matrix4::operator=(const Matrix4 & mat){
            for(int i=0;i<16;i++){
                m_Datas[i]=mat.m_Datas[i];
            }
        }

        void Matrix4::operator =(float * array){
            for(int i=0;i<16;i++){
                m_Datas[i]=array[i];
            }
        }

        Matrix4 & Matrix4::operator*(const Matrix4 & mat2){
            float * array = new float[16];

            for(int i=0; i< 16;i++){
                int val     = i/4;
                int val2    =   i%4;

                array[i]=   (at((val*4))    *mat2.at(val2))         +
                            (at((val*4)+1)  *mat2.at(val2+4))     +
                            (at((val*4)+2)  *mat2.at(val2+8))     +
                            (at((val*4)+3)  *mat2.at(val2+12));
            }

            Matrix4 * mat = new Matrix4(array);
            return *mat;
        }

        vec4 &  Matrix4::operator *(const vec4 & vec){
            vec4 * returnValue = new vec4();

            returnValue->x   =   at(0)*vec.x  + at(1)*vec.y    + at(2)*vec.z    + at(3)*vec.w;
            returnValue->y   =   at(4)*vec.x  + at(5)*vec.y    + at(6)*vec.z    + at(7)*vec.w;
            returnValue->z   =   at(8)*vec.x  + at(9)*vec.y    + at(10)*vec.z   + at(11)*vec.w;
            returnValue->w   =   at(12)*vec.x + at(13)*vec.y   + at(14)*vec.z   + at(15)*vec.w;

            return *returnValue;
        }

        /********************************************
        /*          Methods
        /********************************************/

            float * Matrix4::Data() const {
                float * newArray = new float[16];
                std::copy(&m_Datas[0], &m_Datas[16], newArray);
                return newArray;
            }

            void Matrix4::Print(){
                printf("\n Matrix : \n ");
                for(int i=0;i<4;i++){
                    for(int j=0;j<4;j++){
                        printf("%f, ", m_Datas[i*4+j]);
                    }
                    printf("\n");
                }
            }

            void Matrix4::Identity(){
                m_Datas[0]=1; 	m_Datas[1]=0; 	m_Datas[2]=0; 	m_Datas[3]=0;
                m_Datas[4]=0; 	m_Datas[5]=1; 	m_Datas[6]=0; 	m_Datas[7]=0;
                m_Datas[8]=0; 	m_Datas[9]=0; 	m_Datas[10]=1; 	m_Datas[11]=0;
                m_Datas[12]=0;	m_Datas[13]=0; 	m_Datas[14]=0; 	m_Datas[15]=1;
            }

            void Matrix4::Translate(float x, float y, float z){
                Matrix4 mat;
                mat.Identity();

                mat[3]=x;
                mat[7]=y;
                mat[11]=z;

                (*this)=(*this)*mat;
            }



            void Matrix4::Push(){
                //pile.push(*this);
            }

            void Matrix4::Pop(){
              // *this=pile.top();
               // pile.pop();

            }

            void Matrix4::Rotatex(float angle){
                Matrix4 mat;
                mat.Identity();

                mat[0]=1; 	mat[1]=0; 			mat[2]=0; 			mat[3]=0;
                mat[4]=0; 	mat[5]=cos(angle); 	mat[6]=sin(angle); 	mat[7]=0;
                mat[8]=0; 	mat[9]=-sin(angle); mat[10]=cos(angle); mat[11]=0;
                mat[12]=0;	mat[13]=0; 			mat[14]=0; 			mat[15]=1;

                *this=*this * mat;
            }

            void Matrix4::Rotatey(float angle){
                Matrix4 mat;
                mat.Identity();

                mat[0]=cos(angle); 	mat[1]=0; 	mat[2]=sin(angle); 		mat[3]=0;
                mat[4]=0; 			mat[5]=1; 	mat[6]=0;               mat[7]=0;
                mat[8]=-sin(angle); mat[9]=0; 	mat[10]=cos(angle);     mat[11]=0;
                mat[12]=0;			mat[13]=0; 	mat[14]=0; 				mat[15]=1;

                *this=*this * mat;
            }

            void Matrix4::Rotatez(float angle){
                Matrix4 mat;
                mat.Identity();

                mat[0]=cos(angle); 	mat[1]=sin(angle); 	mat[2]=0;       mat[3]=0;
                mat[4]=-sin(angle); mat[5]=cos(angle); 	mat[6]=0;       mat[7]=0;
                mat[8]=0; 			mat[9]=0; 			mat[10]=1; 		mat[11]=0;
                mat[12]=0;			mat[13]=0; 			mat[14]=0; 		mat[15]=1;

                *this=*this * mat;;
            }

    /********************************************
    /*              Static
    /********************************************/

        /********************************************
        /*              Methods
        /********************************************/

            Matrix4 & Matrix4::Translation(float x, float y, float z){
                Matrix4  * mat = new Matrix4();

                mat->Translate(x,y,z);

                return *mat;
            }

            Matrix4  & Matrix4::Rotation(float x, float y, float z){
                Matrix4 *matx   =new Matrix4();
                Matrix4 *maty   =new Matrix4();
                Matrix4 * matz  =new Matrix4();

                matx->Rotatex(x);
                maty->Rotatey(y);
                matz->Rotatez(z);

                return((*matz)*(*maty)*(*matx));
            }

            Matrix4 & Matrix4::Ortho(float left, float right, float top, float bot, float near, float far){
                Matrix4 & mat = *(new Matrix4());

                mat[0]=(2.0f)/(right-left);  mat[1]=0;                  mat[2]=0;                       mat[3]= -((right+left)/(right-left));
                mat[4]=0;                    mat[5]=(2.0f)/(top-bot);   mat[6]=0;                       mat[7]=-((top+bot)/(top-bot));
                mat[8]=0;                    mat[9]=0;                  mat[10]=(-2.0f)/(far-near);     mat[11]=-((far+near)/(far-near));
                mat[12]=0;                   mat[13]=0;                 mat[14]=0;                      mat[15]=1;

                return mat;
            }

            Matrix4 & Matrix4::Perspective(float angle, float ratio, float near, float far){
                Matrix4 & mat = *(new Matrix4());

                float f= ((1.0) / (tan(angle / 2.0)));

                mat[0]=f/ratio; mat[1]=0; 	mat[2]=0;                            mat[3]=0;
                mat[4]=0; 		mat[5]=f; 	mat[6]=0;                            mat[7]=0;
                mat[8]=0; 		mat[9]=0; 	mat[10]=((near+far)/(near-far));     mat[11]=((2*near*far)/(near-far));
                mat[12]=0;		mat[13]=0; 	mat[14]=-1;                          mat[15]=1;

                return mat;
            }
